#include "total.h"

volatile int control_step = 0;
/*
    -控制模式说明-
    control_step = 0：参数设置
    control_step = 1：A→B, 陀螺仪角度环
    control_step = 2：B→C, 红外位置环
    control_step = 3：C→D, 陀螺仪角度环
    control_step = 4：D→A, 红外位置环
    control_step = 5：A→C, 陀螺仪角度环
    control_step = 6：B→D, 陀螺仪角度环
*/

volatile int Car_Base_Speed;                           //小车基本速度
volatile bool Angle_PIDControl_Flag = false;           //角度环PID控制使能标志位
volatile bool Position_PIDControl_Flag = false;        //位置环PID控制使能标志位
volatile int AnglePID_Result;                          //角度环PID结果
volatile int PositionPID_Result;                       //位置环PID结果
volatile uint16_t counter = 0;                         //计数变量
volatile double Angle_Target;                          //目标角
volatile double err_yaw;                               //误差角
volatile int Mode4_Count = 0;

volatile int test_temp; 

/**
  * @brief  PID控制器初始化函数，声明PID相关句柄
  * @param  无
  * @retval 无
  */
void control_PID_Init(void)
{
    /*角度环PID*/
    hAngle_PID.Init.Setpoint = 0;
	hAngle_PID.Init.Kp = 35;
	hAngle_PID.Init.Ki = 0;
	hAngle_PID.Init.Kd = 0;
	hAngle_PID.Init.OutputUpperLimit = 600;
	hAngle_PID.Init.OutputLowerLimit = -600;
	hAngle_PID.Init.DefaultOutput = 0;
	PAL_PID_Init(&hAngle_PID);      //初始化motorB速度PID控制句柄
	hAngle_PID.Manual = false;	    //关闭手动控制模式

    /*位置环PID*/
    hPosition_PID.Init.Setpoint = 0;
	hPosition_PID.Init.Kp = 150;
	hPosition_PID.Init.Ki = 10;
	hPosition_PID.Init.Kd = 0;
	hPosition_PID.Init.OutputUpperLimit = 600;
	hPosition_PID.Init.OutputLowerLimit = -600;
	hPosition_PID.Init.DefaultOutput = 0;
	PAL_PID_Init(&hPosition_PID);      //初始化motorB速度PID控制句柄
	hPosition_PID.Manual = false;	   //关闭手动控制模式
}


void control_Proc(void)
{
    static int button = 0;
    if(Mode == 0)
    {
        TASK_OLED_DELAY = 20;
        display_OLED_mode = 0;
        button = button_Proc();
        if(button == 1)
        {
            button_position++;
            if(button_position == 6)
            {
                button_position = 1;
            }
        }
        if(button == 2)
        {
            Mode = button_position;
            printf("Mode change\r\n");  //test
            button_position = 1;
            OLED_Clear();
        }
        button = 0;
    }
    if(Mode == 1)
    {
        if(control_step == 0)
        {
            display_OLED_mode = 1;
            TASK_OLED_DELAY = 20;
            button = button_Proc();
            if(button == 1)
            {
                Mode = 0;
                OLED_Clear();
            }
            if(button == 2)
            {
                Angle_Target = wz;
                printf("Set succeed\r\n");  //test
            }
            if(button == 3)
            {
                TASK_OLED_DELAY = 200;
                PAL_PID_Reset(&hAngle_PID);
                PAL_PID_Reset(&hPosition_PID);
                control_step = 1;
                display_OLED_mode = 10;
                OLED_Clear();
                counter = 0;
                Car_Base_Speed = 600;
                printf("Go\r\n");  //test
            } 
            button = 0;
        }
        if(control_step == 1)
        {
            control_mode1();
        }
    }
    if(Mode == 2)
    {
        if(control_step == 0)
        {
            display_OLED_mode = 2;
            TASK_OLED_DELAY = 20;
            button = button_Proc();
            if(button == 1)
            {
                Mode = 0;
                OLED_Clear();
            }
            if(button == 2)
            {
                Angle_Target = wz;
            }
            if(button == 3)
            {
                TASK_OLED_DELAY = 200;
                PAL_PID_Reset(&hAngle_PID);
                PAL_PID_Reset(&hPosition_PID);
                control_step = 1;
                display_OLED_mode = 10;
                OLED_Clear();
                counter = 0;
                Car_Base_Speed = 600;
            } 
            button = 0;
        }
        if(control_step == 1)
        {
            control_mode2();
        }
        if(control_step == 2)
        {
            control_mode3();
        }
        if(control_step == 3)
        {
            control_mode4();
        }
        if(control_step == 4)
        {
            control_mode5();
        }
    }

    if(Mode == 3)
    {
        if(control_step == 0)
        {
            display_OLED_mode = 3;
            TASK_OLED_DELAY = 20;
            button = button_Proc();
            if(button == 1)
            {
                Mode = 0;
                OLED_Clear();
            }
            if(button == 2)
            {
                Angle_Target = wz;
            }
            if(button == 3)
            {
                TASK_OLED_DELAY = 200;
                PAL_PID_Reset(&hAngle_PID);
                PAL_PID_Reset(&hPosition_PID);
                control_step = 1;
                display_OLED_mode = 10;
                OLED_Clear();
                counter = 0;
                Car_Base_Speed = 600;
            } 
            button = 0;
        }
        if(control_step == 1)
        {
            control_mode6();
        }
        if(control_step == 2)
        {
            control_mode7();
        }
        if(control_step == 3)
        {
            control_mode8();
        }
        if(control_step == 4)
        {
            control_mode9();
        }
    }

    if(Mode == 4)
    {
        if(control_step == 0)
        {
            display_OLED_mode = 4;
            TASK_OLED_DELAY = 20;
            button = button_Proc();
            if(button == 1)
            {
                Mode = 0;
                OLED_Clear();
            }
            if(button == 2)
            {
                Angle_Target = wz;
            }
            if(button == 3)
            {
                TASK_OLED_DELAY = 200;
                PAL_PID_Reset(&hAngle_PID);
                PAL_PID_Reset(&hPosition_PID);
                control_step = 1;
                display_OLED_mode = 10;
                OLED_Clear();
                counter = 0;
                Car_Base_Speed = 800;
            } 
            button = 0;
        }
        if(control_step == 1)
        {
            control_mode6();
        }
        if(control_step == 2)
        {
            control_mode7();
        }
        if(control_step == 3)
        {
            control_mode8();
        }
        if(control_step == 4)
        {
            control_mode9();
        }
    }

    if(Mode == 5)
    {
        Car_Base_Speed = 600;
        display_OLED_mode = 5;
        control_Position_PID();
        button = button_Proc();
        if(button == 1)
        {
            Mode = 0;
            OLED_Clear();
        }
        if(button == 2)
        {
            
        }
         if(button == 3)
        {
            
        }
    }
}


/**
  * @brief  电机角度PID控制函数，当PID控制使能标志位置位以后进行PID计算并根据结果设置电机PWM占空比
  * @param  无
  * @retval 无
  */
void control_Angle_PID(void)
{
    if(Angle_PIDControl_Flag == true)
    {
        if((wz - Angle_Target < 180)&&(wz - Angle_Target > -180))
        {
            err_yaw = wz - Angle_Target;
        }
        else if(wz - Angle_Target >= 180)
        {
            err_yaw = wz - Angle_Target - 360;
        }
        else if(wz - Angle_Target <= -180)
        {
            err_yaw = wz - Angle_Target + 360;
        }
        AnglePID_Result = -(int)PAL_PID_Compute1(&hAngle_PID, err_yaw);
        motor_Direction_Set_Auto(Car_Base_Speed + AnglePID_Result, Car_Base_Speed - AnglePID_Result);
        motor_Speed_Set(Car_Base_Speed + AnglePID_Result, Car_Base_Speed - AnglePID_Result);
        printf("A:%f,%f,%d\r\n",Angle_Target,err_yaw,AnglePID_Result);
        Angle_PIDControl_Flag = false;
    }
}


/**
  * @brief  电机位置PID控制函数，当PID控制使能标志位置位以后进行PID计算并根据结果设置电机PWM占空比
  * @param  无
  * @retval 无
  */
void control_Position_PID(void)
{
    if(Position_PIDControl_Flag == true)
    {
        // int tracking_out = tracking_Enable();
        int tracking_out = tracking_Digital();
        PositionPID_Result = -(int)PAL_PID_Compute1(&hPosition_PID, tracking_out);
        motor_Direction_Set_Auto(Car_Base_Speed + PositionPID_Result, Car_Base_Speed - PositionPID_Result);
        motor_Speed_Set(Car_Base_Speed + PositionPID_Result, Car_Base_Speed - PositionPID_Result);
        printf("P:%d,%d\r\n",tracking_out,PositionPID_Result);
        Position_PIDControl_Flag = false;
    }
}


/**
  * @brief  控制模式1，A点直线行驶至B点，并停车
  * @param  无
  * @retval 无
  */
void control_mode1(void)
{
    control_Angle_PID();
    tracking_Digital();
    if(H0 == 1||H1 == 1||H2 == 1||H3 == 1||H4 == 1||H5 == 1||H6 == 1)
    {
        printf("black\r\n");  //test
        counter++;
        if(counter > 100)
        {
            counter = 0;
            motor_Direction_Set(0, 0);
            display_Remind_flag = 1;
            Mode = 0;
            control_step = 0;
            OLED_Clear();
            display_OLED_mode = 0;
        }
    }
}


/**
  * @brief  控制模式2，A点直线行驶至B点，不停车
  * @param  无
  * @retval 无
  */
void control_mode2(void)
{
    control_Angle_PID();
    tracking_Digital();
    if(H0 == 1||H1 == 1||H2 == 1||H3 == 1||H4 == 1||H5 == 1||H6 == 1)
    {
        printf("black\r\n");
        counter++;
        if(counter > 100)
        {
            counter = 0;
            display_Remind_flag = 2;
            control_step = 2;
            Angle_Target = -177.5;
        }
    }
}


/**
  * @brief  控制模式3，B点巡线至C点，不停车
  * @param  无
  * @retval 无
  */
void control_mode3(void)
{
    control_Position_PID();

    // if((wz - Angle_Target < 180)&&(wz - Angle_Target > -180))
    // {
    //     err_yaw = wz - Angle_Target;
    // }
    // else if(wz - Angle_Target >= 180)
    // {
    //     err_yaw = wz - Angle_Target - 360;
    // }
    // else if(wz - Angle_Target <= -180)
    // {
    //     err_yaw = wz - Angle_Target + 360;
    // }

    if(H0 == 0 && H1 == 0 && H2 == 0 && H3 == 0 && H4 == 0 && H5 == 0 && H6 == 0)
    {
        printf("Write!!!\r\n");     //test
        counter++;
        if(counter > 50)
        {
            counter = 0;
            display_Remind_flag = 2;
            control_step = 3;
            
        }
    }
}


/**
  * @brief  控制模式4，C点直线行驶至D点,不停车
  * @param  无
  * @retval 无
  */
 void control_mode4(void)
 {
    control_Angle_PID();
    tracking_Digital();
    if(H0 == 1||H1 == 1||H2 == 1||H3 == 1||H4 == 1||H5 == 1||H6 == 1)
    {
        counter++;
        if(counter > 100)
        {
            counter = 0;
            display_Remind_flag = 2;
            control_step = 4;
            Angle_Target = 0;
        }
    }
 }


 /**
  * @brief  控制模式5，D点巡线至A点，并停车
  * @param  无
  * @retval 无
  */
void control_mode5(void)
{
    control_Position_PID();

    // if((wz - Angle_Target < 180)&&(wz - Angle_Target > -180))
    // {
    //     err_yaw = wz - Angle_Target;
    // }
    // else if(wz - Angle_Target >= 180)
    // {
    //     err_yaw = wz - Angle_Target - 360;
    // }
    // else if(wz - Angle_Target <= -180)
    // {
    //     err_yaw = wz - Angle_Target + 360;
    // }

    if(H0 == 0 && H1 == 0 && H2 == 0 && H3 == 0 && H4 == 0 && H5 == 0 && H6 == 0)
    {
        counter++;
        if(counter > 100)
        {
            motor_Direction_Set(0, 0);
            counter = 0;
            display_Remind_flag = 1;
            control_step = 0;
        }
    }
}


/**
  * @brief  控制模式6，A点直线行驶至C点，不停车
  * @param  无
  * @retval 无
  */
void control_mode6(void)
{
    control_Angle_PID();
    tracking_Digital();
    if(H0 == 1||H1 == 1||H2 == 1||H3 == 1||H4 == 1||H5 == 1||H6 == 1)
    {
        printf("black\r\n");
        counter++;
        if(counter > 100)
        {
            counter = 0;
            display_Remind_flag = 2;
            control_step = 2;
            Angle_Target = -149;
        }
    }
}


/**
  * @brief  控制模式7，C点巡线至B点，不停车
  * @param  无
  * @retval 无
  */
void control_mode7(void)
{
    control_Position_PID();

    // if((wz - Angle_Target < 180)&&(wz - Angle_Target > -180))
    // {
    //     err_yaw = wz - Angle_Target;
    // }
    // else if(wz - Angle_Target >= 180)
    // {
    //     err_yaw = wz - Angle_Target - 360;
    // }
    // else if(wz - Angle_Target <= -180)
    // {
    //     err_yaw = wz - Angle_Target + 360;
    // }
    if(H0 == 0 && H1 == 0 && H2 == 0 && H3 == 0 && H4 == 0 && H5 == 0 && H6 == 0)
    {
        printf("Write\r\n");
        counter++;
        if(counter > 100)
        {
            counter = 0;
            display_Remind_flag = 2;
            control_step = 3;
        }
    }
}


/**
  * @brief  控制模式8，B点直线行驶至D点，不停车
  * @param  无
  * @retval 无
  */
void control_mode8(void)
{
    control_Angle_PID();
    tracking_Digital();
    if(H0 == 1||H1 == 1||H2 == 1||H3 == 1||H4 == 1||H5 == 1||H6 == 1)
    {
        printf("black\r\n");
        counter++;
        if(counter > 100)
        {
            counter = 0;
            display_Remind_flag = 2;
            control_step = 4;
            Angle_Target = 0;
        }
    }
}


/**
  * @brief  控制模式9，D点巡线至A点，并停车
  * @param  无
  * @retval 无
  */
void control_mode9(void)
{
    control_Position_PID();

    if((wz - Angle_Target < 180)&&(wz - Angle_Target > -180))
    {
        err_yaw = wz - Angle_Target;
    }
    else if(wz - Angle_Target >= 180)
    {
        err_yaw = wz - Angle_Target - 360;
    }
    else if(wz - Angle_Target <= -180)
    {
        err_yaw = wz - Angle_Target + 360;
    }

    if(H0 == 0 && H1 == 0 && H2 == 0 && H3 == 0 && H4 == 0 && H5 == 0 && H6 == 0)
    {
        counter++;
        if(counter > 100)
        {
            if(Mode == 3)
            {
                motor_Direction_Set(0, 0);
                counter = 0;
                display_Remind_flag = 1;
                control_step = 0;
            }
            else if(Mode == 4)
            {
                Mode4_Count++;
                if(Mode4_Count == 4)
                {
                    motor_Direction_Set(0, 0);
                    counter = 0;
                    display_Remind_flag = 1;
                    control_step = 0;
                }
                else 
                {
                    counter = 0;
                    display_Remind_flag = 2;
                    control_step = 1;
                    Angle_Target = -36;
                }
                
            }
        }
    }
}